/**
 * Copyright (c) 2009 blurredVision
 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and
 * associated documentation files (the "Software"), to deal in the Software without restriction, 
 * including without limitation the rights to use, copy, modify, merge, publish, distribute, 
 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in all copies or 
 * substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT 
 * NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT 
 * OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */
#include "StdAfx.h"
#include "Frustum.h"
#include "core/core.h"

Frustum::Frustum(void)
{
}

Frustum::~Frustum(void)
{
}

void Frustum::calculateFrustum()
{
	math::Matrix4 clip = coreGetRenderer()->getWorldViewProjMatrix();
	//clip.print();
	
	// Now we actually want to get the sides of the frustum.  To do this we take
	// the clipping planes we received above and extract the sides from them.

	// This will extract the RIGHT side of the frustum
	m_fFrustum[RIGHT][A] = clip.m[ 3] - clip.m[ 0];
	m_fFrustum[RIGHT][B] = clip.m[ 7] - clip.m[ 4];
	m_fFrustum[RIGHT][C] = clip.m[11] - clip.m[ 8];
	m_fFrustum[RIGHT][D] = clip.m[15] - clip.m[12];
	// Normalize the RIGHT side
	normalizePlane(RIGHT);

	// This will extract the LEFT side of the frustum
	m_fFrustum[LEFT][A] = clip.m[ 3] + clip.m[ 0];
	m_fFrustum[LEFT][B] = clip.m[ 7] + clip.m[ 4];
	m_fFrustum[LEFT][C] = clip.m[11] + clip.m[ 8];
	m_fFrustum[LEFT][D] = clip.m[15] + clip.m[12];
	// Normalize the LEFT side
	normalizePlane(LEFT);

	// This will extract the BOTTOM side of the frustum
	m_fFrustum[BOTTOM][A] = clip.m[ 3] + clip.m[ 1];
	m_fFrustum[BOTTOM][B] = clip.m[ 7] + clip.m[ 5];
	m_fFrustum[BOTTOM][C] = clip.m[11] + clip.m[ 9];
	m_fFrustum[BOTTOM][D] = clip.m[15] + clip.m[13];
	// Normalize the BOTTOM side
	normalizePlane(BOTTOM);

	// This will extract the TOP side of the frustum
	m_fFrustum[TOP][A] = clip.m[ 3] - clip.m[ 1];
	m_fFrustum[TOP][B] = clip.m[ 7] - clip.m[ 5];
	m_fFrustum[TOP][C] = clip.m[11] - clip.m[ 9];
	m_fFrustum[TOP][D] = clip.m[15] - clip.m[13];
	// Normalize the TOP side
	normalizePlane(TOP);

	// This will extract the BACK side of the frustum
	m_fFrustum[BACK][A] = clip.m[ 3] - clip.m[ 2];
	m_fFrustum[BACK][B] = clip.m[ 7] - clip.m[ 6];
	m_fFrustum[BACK][C] = clip.m[11] - clip.m[10];
	m_fFrustum[BACK][D] = clip.m[15] - clip.m[14];
	// Normalize the BACK side
	normalizePlane(BACK);

	// This will extract the FRONT side of the frustum
	m_fFrustum[FRONT][A] = clip.m[ 3] + clip.m[ 2];
	m_fFrustum[FRONT][B] = clip.m[ 7] + clip.m[ 6];
	m_fFrustum[FRONT][C] = clip.m[11] + clip.m[10];
	m_fFrustum[FRONT][D] = clip.m[15] + clip.m[14];
	// Normalize the FRONT side
	normalizePlane(FRONT);
}

void Frustum::normalizePlane(int side)
{
	// Here we calculate the magnitude of the normal to the plane (point A B C)
	// Remember that (A, B, C) is that same thing as the normal's (X, Y, Z).
	// To calculate magnitude you use the equation:  magnitude = sqrt( x^2 + y^2 + z^2)
	float magnitude = 1.0f / (float)sqrt( m_fFrustum[side][A] * m_fFrustum[side][A] +
								          m_fFrustum[side][B] * m_fFrustum[side][B] +
								          m_fFrustum[side][C] * m_fFrustum[side][C] );

	// Then we divide the plane's values by it's magnitude.
	// This makes it easier to work with.
	m_fFrustum[side][A] *= magnitude;
	m_fFrustum[side][B] *= magnitude;
	m_fFrustum[side][C] *= magnitude;
	m_fFrustum[side][D] *= magnitude;
}

bool Frustum::pointInFrustum(const math::Vector3 &point)
{
	for(int i=0; i<6; ++i) {
		// Calculate the plane equation and check if the point is behind a side of the frustum
		if( m_fFrustum[i][A]*point.x + 
			m_fFrustum[i][B]*point.y + 
			m_fFrustum[i][C]*point.z + 
			m_fFrustum[i][D] <= 0) {
			return false;
		}
	}
	return true;
}

bool Frustum::sphereInFrustum(const model::BoundingSphere &sphere)
{
	for(int i=0; i<6; ++i)	{
		math::Vector3 center = sphere.getCenter();
		// If the center of the sphere is farther away from the plane than the radius
		if( m_fFrustum[i][A]*center.x + 
			m_fFrustum[i][B]*center.y + 
			m_fFrustum[i][C]*center.z + 
			m_fFrustum[i][D] <= -sphere.getRadius() ) {
			return false;
		}
	}
	return true;
}

bool Frustum::boxInFrustum(const model::AABoundingBox &box)
{
	/*
	for (int i=0; i<6; ++i) {
		if (m_fFrustum[i][A] * box.getMin().x + m_fFrustum[i][B] * box.getMin().y + m_fFrustum[i][C] * box.getMin().z + m_fFrustum[i][D] > 0) 
			continue;
		if (m_fFrustum[i][A] * box.getMax().x + m_fFrustum[i][B] * box.getMin().y + m_fFrustum[i][C] * box.getMin().z + m_fFrustum[i][D] > 0) 
				continue;
		if (m_fFrustum[i][A] * box.getMin().x + m_fFrustum[i][B] * box.getMax().y + m_fFrustum[i][C] * box.getMin().z + m_fFrustum[i][D] > 0) 
			continue;
		if (m_fFrustum[i][A] * box.getMax().x + m_fFrustum[i][B] * box.getMax().y + m_fFrustum[i][C] * box.getMin().z + m_fFrustum[i][D] > 0) 
				continue;
		if (m_fFrustum[i][A] * box.getMin().x + m_fFrustum[i][B] * box.getMin().y + m_fFrustum[i][C] * box.getMax().z + m_fFrustum[i][D] > 0) 
			continue;
		if (m_fFrustum[i][A] * box.getMax().x + m_fFrustum[i][B] * box.getMin().y + m_fFrustum[i][C] * box.getMax().z + m_fFrustum[i][D] > 0) 
				continue;
		if (m_fFrustum[i][A] * box.getMin().x + m_fFrustum[i][B] * box.getMax().y + m_fFrustum[i][C] * box.getMax().z + m_fFrustum[i][D] > 0) 
			continue;
		if (m_fFrustum[i][A] * box.getMax().x + m_fFrustum[i][B] * box.getMax().y + m_fFrustum[i][C] * box.getMax().z + m_fFrustum[i][D] > 0) 
				continue;

		return false;
	}
	return true;
	*/

	float m, n; int i;
	bool result = true;
	math::Vector3 dim = box.getMax() - box.getMin();	

	for (i = 0; i < 6; ++i) { 
		 m = (box.getMin().x * m_fFrustum[i][A]) + (box.getMin().y * m_fFrustum[i][B]) + (box.getMin().z * m_fFrustum[i][C]) + m_fFrustum[i][D];
		 n = (dim.x * fabs(m_fFrustum[i][A])) + (dim.y * fabs(m_fFrustum[i][B])) + (dim.z * fabs(m_fFrustum[i][C]));

		 if (m + n < 0) return false;
		 if (m - n < 0) result = true;

     } 
	return result;
}

float* Frustum::getFrustum()
{
	return &m_fFrustum[0][0];
}